package tests;

import java.awt.AWTException;
import java.awt.Robot;

import SimpleOpenNI.*;
import processing.core.*;
import msafluid.*;

import processing.opengl.*;
import javax.media.opengl.*;

import java.nio.FloatBuffer;
import com.sun.opengl.util.*;

public class exa1 extends PApplet {
	final float FLUID_WIDTH = 120;
	boolean renderUsingVA = true;

	float invWidth, invHeight;    // inverse of screen dimensions
	float aspectRatio, aspectRatio2;

	MSAFluidSolver2D fluidSolver;

	ParticleSystem particleSystem;

	PImage imgFluid;

	boolean drawFluid = true;

	public SimpleOpenNI  context;
	PImage img;
	
	void fadeToColor(GL gl, float r, float g, float b, float speed) {
	    gl.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA);
	    gl.glColor4f(r, g, b, speed);
	    gl.glBegin(GL.GL_QUADS);
	    gl.glVertex2f(0, 0);
	    gl.glVertex2f(width, 0);
	    gl.glVertex2f(width, height);
	    gl.glVertex2f(0, height);
	    gl.glEnd();
	}

	public void setup()
	{
		//fluid
	    hint( ENABLE_OPENGL_4X_SMOOTH );    // Turn on 4X antialiasing

	    invWidth = 1.0f/width;
	    invHeight = 1.0f/height;
	    aspectRatio = width * invHeight;
	    aspectRatio2 = aspectRatio * aspectRatio;
	    
		fluidSolver = new MSAFluidSolver2D((int)(FLUID_WIDTH), (int)(FLUID_WIDTH * height/width));
	    fluidSolver.enableRGB(true).setFadeSpeed(-0.001F).setDeltaT(0.1F).setVisc(0F);
	    img = createImage( fluidSolver.getWidth(), fluidSolver.getHeight(), RGB );
	    // create image to hold fluid picture
	    imgFluid = createImage(fluidSolver.getWidth(), fluidSolver.getHeight(), RGB);

	    // create particle system
	    particleSystem = new ParticleSystem();
	    
		//fluid
		
	 // context = new SimpleOpenNI(this);
	  context = new SimpleOpenNI(this,SimpleOpenNI.RUN_MODE_MULTI_THREADED);
	  
	  // enable depthMap generation 
	  context.enableDepth();
	  
	  // enable skeleton generation for all joints
	  context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);

	  background(200,0,0);
	  stroke(0,0,255);
	  strokeWeight(3);
	  smooth();
	  
	  size(context.depthWidth(), context.depthHeight(), OPENGL); 
	}
	
	public void mouseMoved() {
	    float mouseNormX = mouseX * invWidth;
	    float mouseNormY = mouseY * invHeight;
	    float mouseVelX = (mouseX - pmouseX) * invWidth;
	    float mouseVelY = (mouseY - pmouseY) * invHeight;

	    //addForce(mouseNormX, mouseNormY, mouseVelX, mouseVelY);
	}

	public void draw()
	{
		//fluid
			  
		  fluidSolver.update();
		  //img.loadPixels();
		  for(int i=0; i<fluidSolver.getNumCells(); i++) {
	            int d = 2;
	            imgFluid.pixels[i] = color(fluidSolver.r[i] * d, fluidSolver.g[i] * d, fluidSolver.b[i] * d);
	        }  
	        imgFluid.updatePixels();//  fastblur(imgFluid, 2);
	        image(imgFluid, 0, 0, width, height);
	        particleSystem.updateAndDraw();
		//fluid
		
	  // update the cam
	  context.update();
	  context.setMirror(true);
	  //image(context.rgbImage(),0,0);
	  
	  // draw the skeleton if it's available
	  try {
		  
		  if(context.isTrackingSkeleton(1)){
		    drawSkeleton(1);
		  	drawCycle(1);
		  }
		  if(context.isTrackingSkeleton(2)){
			drawSkeleton(2);
		  	drawCycle(2);
		  }
		  if(context.isTrackingSkeleton(3)){
			drawSkeleton(3);
		  	drawCycle(3);
		  }
	
			
		} catch (AWTException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		
	}
	
	public void drawCycle(int pers_no) throws AWTException{
		PVector hand_real = new PVector();
		PVector hand_pers = new PVector();

		
		context.getJointPositionSkeleton(pers_no, SimpleOpenNI.SKEL_LEFT_HAND, hand_real);

		
		context.convertRealWorldToProjective(hand_real, hand_pers);
		ellipse(hand_pers.x, hand_pers.y, 40, 40);
		addForce(hand_pers.x, hand_pers.y, 0.1F, 0.1F);
//		System.out.println("rot" + pers_no);
			
	}

	// draw the skeleton with the selected joints
	public void drawSkeleton(int userId)
	{
	  
	  context.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

	  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

	  context.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

	  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

	  context.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
	  context.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);  
	}

	// -----------------------------------------------------------------
	// SimpleOpenNI events

	public void onNewUser(int userId)
	{
	  println("onNewUser - userId: " + userId);
	  println("  start pose detection");
	  
	  context.startPoseDetection("Psi",userId);
	}

	public void onLostUser(int userId)
	{
	  println("onLostUser - userId: " + userId);
	}

	public void onStartCalibration(int userId)
	{
	  println("onStartCalibration - userId: " + userId);
	}

	public void onEndCalibration(int userId, boolean successfull)
	{
	  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);
	  
	  if (successfull) 
	  { 
	    println("  User calibrated !!!");
	    context.startTrackingSkeleton(userId); 
	  } 
	  else 
	  { 
	    println("  Failed to calibrate user !!!");
	    println("  Start pose detection");
	    context.startPoseDetection("Psi",userId);
	  }
	}

	public void onStartPose(String pose,int userId)
	{
	  println("onStartPose - userId: " + userId + ", pose: " + pose);
	  println(" stop pose detection");
	  
	  context.stopPoseDetection(userId); 
	  context.requestCalibrationSkeleton(userId, true);
	 
	}

	public void onEndPose(String pose,int userId)
	{
	  println("onEndPose - userId: " + userId + ", pose: " + pose);
	}
	
	class ParticleSystem {
	    FloatBuffer posArray;
	    FloatBuffer colArray;

	    final static int maxParticles = 5000;
	    int curIndex;

	    Particle[] particles;

	    ParticleSystem() {
	        particles = new Particle[maxParticles];
	        for(int i=0; i<maxParticles; i++) particles[i] = new Particle();
	        curIndex = 0;

	        posArray = BufferUtil.newFloatBuffer(maxParticles * 2 * 2);// 2 coordinates per point, 2 points per particle (current and previous)
	        colArray = BufferUtil.newFloatBuffer(maxParticles * 3 * 2);
	    }


	    void updateAndDraw(){
	        PGraphicsOpenGL pgl = (PGraphicsOpenGL) g;         // processings opengl graphics object
	        GL gl = pgl.beginGL();                // JOGL's GL object

	        gl.glEnable( GL.GL_BLEND );             // enable blending
	        if(!drawFluid) fadeToColor(gl, 0, 0, 0, 0.05F);

	        gl.glBlendFunc(GL.GL_ONE, GL.GL_ONE);  // additive blending (ignore alpha)
	        gl.glEnable(GL.GL_LINE_SMOOTH);        // make points round
	        gl.glLineWidth(1);


	        if(renderUsingVA) {
	            for(int i=0; i<maxParticles; i++) {
	                if(particles[i].alpha > 0) {
	                    particles[i].update();
	                    particles[i].updateVertexArrays(i, posArray, colArray);
	                }
	            }    
	            gl.glEnableClientState(GL.GL_VERTEX_ARRAY);
	            gl.glVertexPointer(2, GL.GL_FLOAT, 0, posArray);

	            gl.glEnableClientState(GL.GL_COLOR_ARRAY);
	            gl.glColorPointer(3, GL.GL_FLOAT, 0, colArray);

	            gl.glDrawArrays(GL.GL_LINES, 0, maxParticles * 2);
	        } 
	        else {
	            gl.glBegin(GL.GL_LINES);               // start drawing points
	            for(int i=0; i<maxParticles; i++) {
	                if(particles[i].alpha > 0) {
	                    particles[i].update();
	                    particles[i].drawOldSchool(gl);    // use oldschool renderng
	                }
	            }
	            gl.glEnd();
	        }

	        gl.glDisable(GL.GL_BLEND);
	        pgl.endGL();
	    }


	    void addParticles(float x, float y, int count ){
	        for(int i=0; i<count; i++) addParticle(x + random(-15, 15), y + random(-15, 15));
	    }


	    void addParticle(float x, float y) {
	        particles[curIndex].init(x, y);
	        curIndex++;
	        if(curIndex >= maxParticles) curIndex = 0;
	    }

	}
	
	class Particle {
	    final static float MOMENTUM = 0.5F;
	    final static float FLUID_FORCE = 0.6F;

	    float x, y;
	    float vx, vy;
	    float radius;       // particle's size
	    float alpha;
	    float mass;

	    void init(float x, float y) {
	        this.x = x;
	        this.y = y;
	        vx = 0;
	        vy = 0;
	        radius = 5;
	        alpha  = random(0.3F, 1);
	        mass = random(0.1F, 1);
	    }


	    void update() {
	        // only update if particle is visible
	        if(alpha == 0) return;

	        // read fluid info and add to velocity
	        int fluidIndex = fluidSolver.getIndexForNormalizedPosition(x * invWidth, y * invHeight);
	        vx = fluidSolver.u[fluidIndex] * width * mass * FLUID_FORCE + vx * MOMENTUM;
	        vy = fluidSolver.v[fluidIndex] * height * mass * FLUID_FORCE + vy * MOMENTUM;

	        // update position
	        x += vx;
	        y += vy;

	        // bounce of edges
	        if(x<0) {
	            x = 0;
	            vx *= -1;
	        }
	        else if(x > width) {
	            x = width;
	            vx *= -1;
	        }

	        if(y<0) {
	            y = 0;
	            vy *= -1;
	        }
	        else if(y > height) {
	            y = height;
	            vy *= -1;
	        }

	        // hackish way to make particles glitter when the slow down a lot
	        if(vx * vx + vy * vy < 1) {
	            vx = random(-1, 1);
	            vy = random(-1, 1);
	        }

	        // fade out a bit (and kill if alpha == 0);
	        alpha *= 0.999;
	        if(alpha < 0.01) alpha = 0;

	    }


	    void updateVertexArrays(int i, FloatBuffer posBuffer, FloatBuffer colBuffer) {
	        int vi = i * 4;
	        posBuffer.put(vi++, x - vx);
	        posBuffer.put(vi++, y - vy);
	        posBuffer.put(vi++, x);
	        posBuffer.put(vi++, y);

	        int ci = i * 6;
	        colBuffer.put(ci++, alpha);
	        colBuffer.put(ci++, alpha);
	        colBuffer.put(ci++, alpha);
	        colBuffer.put(ci++, alpha);
	        colBuffer.put(ci++, alpha);
	        colBuffer.put(ci++, alpha);
	    }


	    void drawOldSchool(GL gl) {
	        gl.glColor3f(alpha, alpha, alpha);
	        gl.glVertex2f(x-vx, y-vy);
	        gl.glVertex2f(x, y);
	    }

	}
	
	void addForce(float x, float y, float dx, float dy) {
	    float speed = dx * dx  + dy * dy * aspectRatio2;    // balance the x and y components of speed with the screen aspect ratio

	    if(speed > 0) {
	        if(x<0) x = 0; 
	        else if(x>1) x = 1;
	        if(y<0) y = 0; 
	        else if(y>1) y = 1;

	        float colorMult = 5;
	        float velocityMult = 30.0f;

	        int index = fluidSolver.getIndexForNormalizedPosition(x, y);

	        int drawColor;

	        colorMode(HSB, 360, 1, 1);
	        float hue = ((x + y) * 180 + frameCount) % 360;
	        drawColor = color(hue, 1, 1);
	        colorMode(RGB, 1);  

	        fluidSolver.rOld[index]  += red(drawColor) * colorMult;
	        fluidSolver.gOld[index]  += green(drawColor) * colorMult;
	        fluidSolver.bOld[index]  += blue(drawColor) * colorMult;

	        particleSystem.addParticles(x * width, y * height, 10);
	        fluidSolver.uOld[index] += dx * velocityMult;
	        fluidSolver.vOld[index] += dy * velocityMult;
	    }
	}
	
}
